#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_bt.h"

#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_bt_main.h"
#include "gatts_table_creat_demo.h"
#include "mybluetooth.h"

#include <stdio.h>
#include "driver/i2c.h"

#ifdef USE_HX711
#include "HX711.h"
#else
#include "ADS1115.h"
#endif

#include "driver/touch_pad.h"

#include "mydelay.h"

#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

#define APPMAIN_TAG "MAIN"

//创建一个file
uint16_t heart_rate_handle_table[HRS_IDX_NB];

float pitch,yaw,roll;

TaskHandle_t *pTask=NULL;
bool notify_flag=false;

//触摸传感器定义
#define TOUCH_BUTTON_NUM 4

static const touch_pad_t button[TOUCH_BUTTON_NUM] = {//四个触摸传感口
    TOUCH_PAD_NUM1,
    TOUCH_PAD_NUM2,
    TOUCH_PAD_NUM3,
    TOUCH_PAD_NUM4,
};

uint32_t touch_ref[4]={};

//触摸传感器初始化
void touch_use()
{
    touch_pad_init();
    for(int i=0;i<TOUCH_BUTTON_NUM;++i){
        touch_pad_config(button[i]);  
    }
    touch_pad_set_fsm_mode(TOUCH_FSM_MODE_TIMER);
    touch_pad_fsm_start();
    delay_ms(40);
    touch_pad_filter_enable();
    delay_ms(40);
    for (int i=0;i<4;++i){
        touch_pad_filter_read_smooth(button[i],&touch_ref[i]);
    }
}

//发送数据计数器
int send_count=0;
uint32_t threshold=1000;//修改这个值以判断触摸传感器有没有接触到皮肤
//在这里采集数据并发送通知
#ifdef USE_HX711  //*****************************************USE_HX711的定义在mybluetooth.h中***************************************************
static void get_val(void *arg){
    unsigned char buf[10]={0};//报头+1个应变传感器数值+触摸传感器状态+姿态角+空字节+报尾
    buf[0]=0xFF;
    buf[9]=0xFE;
    uint16_t temp[4];
    // unsigned char *ptr;
    int16_t temp2=0;                            
    uint32_t  temp3=0;
    TickType_t start_ticks,end_ticks;
    uint32_t period=0;
    while(1){
        start_ticks = xTaskGetTickCount();
        if (notify_flag==true){

            while(mpu_dmp_get_data(&pitch, &roll, &yaw)!=0){};//把溢出的数据读取掉，不然会报错
            mpu_dmp_get_data(&pitch, &roll, &yaw);



            temp2=(int16_t)((get_weight(0)-get_weight(1))*10);//获取应变传感器数值
            // temp2=(int16_t)((get_weight(1))*10);//获取应变传感器数值

            buf[2]=temp2&255;
            buf[1]=(temp2>>8)&255;
            



            buf[3]=0;
            for(int i=0;i<TOUCH_BUTTON_NUM;++i){
                touch_pad_filter_read_smooth(button[i],&temp3);//获取触摸传感器数值
                temp3-=touch_ref[i];
                if (temp3>threshold){
                    buf[3]|=1<<i;
                    temp[i]=1;
                }
                else {
                    temp[i]=0;
                }
                // ESP_LOGI(APPMAIN_TAG, "touch[%d]:%d ", i,temp3);
            }

            // ESP_LOGI("TESTNUM","AD:%f TOUCH:%hu %hu %hu %hu pitch:%.1f roll:%.1f yaw:%.1f",(float)(temp2)/10,temp[0],temp[1],temp[2],temp[3],pitch,roll,yaw);
            pitch+=180.0;
            roll+=180.0;
            yaw+=180.0;
            temp3=(int)pitch;
            buf[4]=((temp3)>>2)&255;
            buf[5]=(((temp3)>>0)&0x03)<<6;
            temp3=(int)roll;
            buf[5]|=(temp3>>4)&0x3F;
            buf[6]=((temp3>>0)&255)<<4;
            temp3=(int)yaw;
            buf[6]|=(((temp3)>>6)&255);
            buf[7]=(((temp3)>>0)&0x3F)<<2;

            // ESP_LOGI(APPMAIN_TAG, "MPU use %lu ", pdTICKS_TO_MS(end_ticks-start_ticks));

            //发送数据
            esp_ble_gatts_set_attr_value(heart_rate_handle_table[IDX_CHAR_VAL_A], 1, buf);
            esp_ble_gatts_send_indicate(heart_rate_profile_tab[0].gatts_if, heart_rate_profile_tab[0].conn_id,
                                                        heart_rate_handle_table[IDX_CHAR_VAL_A], sizeof(buf), buf, false);
            send_count++;


            

            if (send_count>=160){
                notify_flag=false;
            }
        }
        end_ticks = xTaskGetTickCount();
        // ESP_LOGI("TESTNUM","%lu ms ",pdTICKS_TO_MS(end_ticks-start_ticks));
        period=50-pdTICKS_TO_MS(end_ticks-start_ticks);
        // ESP_LOGI("TESTNUM","%lu",period);
        if (period<=50&&period>0){
            delay_ms(period);
            // ESP_LOGI("TESTNUM","1");
        }
        else {
            delay_ms(50);
            // ESP_LOGI("TESTNUM","00000000000000000000000");
        }
        // delay_ms(period>0?period);//
    }
}

#else
static void get_val(void *arg){
    unsigned char buf[10]={0};
    uint16_t temp=0;
    while(1){
        if (notify_flag==true){
            for(int i=0;i<TOUCH_BUTTON_NUM;++i){
                touch_pad_filter_read_smooth(button[i],&temp);
                buf[i<<1]=temp&255;
                buf[i<<1|1]=temp>>8;
                ESP_LOGI(APPMAIN_TAG, "touch[%d]:%hd ", i,temp);
            }
            temp=ADS1115_ReadVoltage(0);
            ESP_LOGI(APPMAIN_TAG, "AD:%hd\n ",temp);
            buf[8]=temp&255;
            buf[9]=temp>>8;

            esp_ble_gatts_set_attr_value(heart_rate_handle_table[IDX_CHAR_VAL_A], 1, &buf);
            esp_ble_gatts_send_indicate(heart_rate_profile_tab[0].gatts_if, heart_rate_profile_tab[0].conn_id,
                                                        heart_rate_handle_table[IDX_CHAR_VAL_A], sizeof(buf), buf, false);
            send_count++;
            if (send_count>=100){
                notify_flag=false;
            }
        }
        
        delay_ms(50);
    }
}
#endif


void app_main(void)
{
    esp_err_t ret;
    //初始化应变传感器或ADS1115
    #ifdef USE_HX711
    HX711_init();
    #else
    i2c_master_init();    
    ADS1115_SetGain(GAIN_TWO);
    ADS1115_SetCompQue(COMPQUE_NONE);
    ADS1115_SetCompLat(COMPLAT_NONLAT);
    DADS1115_SetCompPol(COMPPOL_LOW);
    ADS1115_SetCompMode(COMPMODE_TRAD);
    ADS1115_SetRate(RATE_475); 
    #endif
    //初始化触摸传感器
    touch_use();
    /* Initialize NVS. */
    ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK( ret );
    ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT));
    //初始化蓝牙
    ble_init();

    i2c_master_init();
    delay_ms(500);
    
    ret=MPU_Init();
    ESP_LOGI("MAIN","ret=%d",ret);
    do {
        ret=mpu_dmp_init();
        ESP_LOGI("MAIN","ret=%d",ret);
        delay_ms(1000);
    }while(ret);

    //创建数据发送任务
    xTaskCreate(get_val,"get_val",8192,NULL,10,pTask);
}
